﻿#include "pid.h"
#include <chrono>

/*Constructor (...)*********************************************************
 *    The parameters specified here are those for for which we can't set up
 *    reliable defaults, so we need to have the user set them.
 ***************************************************************************/
PID::PID(double* Input, double* Output, double* Setpoint,
        double Kp, double Ki, double Kd, int POn, int ControllerDirection)
{
    m_output = Output;
    m_input = Input;
    m_setpoint = Setpoint;
    m_inAuto = false;

    PID::SetOutputLimits(0, 255);                //default output limit corresponds to
                                                //the arduino pwm limits

    m_sampleTime = 100;                            //default Controller Sample Time is 0.1 seconds

    PID::SetControllerDirection(ControllerDirection);
    PID::SetTunings(Kp, Ki, Kd, POn);

    m_lastTime = (int64_t)(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count());
}

/*Constructor (...)*********************************************************
 *    To allow backwards compatability for v1.1, or for people that just want
 *    to use Proportional on Error without explicitly saying so
 ***************************************************************************/

PID::PID(double* Input, double* Output, double* Setpoint,
        double Kp, double Ki, double Kd, int ControllerDirection)
    :PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection)
{

}


/* Compute() **********************************************************************
 *     This, as they say, is where the magic happens.  this function should be called
 *   every time "void loop()" executes.  the function will decide for itself whether a new
 *   pid Output needs to be computed.  returns true when the output is computed,
 *   false when nothing has been done.
 **********************************************************************************/
bool PID::Compute()
{
   if(!m_inAuto) return false;
   unsigned long now = (int64_t)(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count());
   unsigned long timeChange = (now - m_lastTime);
   if(timeChange>=m_sampleTime)
   {
      /*Compute all the working error variables*/
      double input = *m_input;
      double error = *m_setpoint - input;
      double dInput = (input - m_lastInput);
      m_outputSum+= (m_ki * error);

      /*Add Proportional on Measurement, if P_ON_M is specified*/
      if(!m_pOnE) m_outputSum-= m_kp * dInput;

      if(m_outputSum > m_outMax) m_outputSum= m_outMax;
      else if(m_outputSum < m_outMin) m_outputSum= m_outMin;

      /*Add Proportional on Error, if P_ON_E is specified*/
       double output;
      if(m_pOnE) output = m_kp * error;
      else output = 0;

      /*Compute Rest of PID Output*/
      output += m_outputSum - m_kd * dInput;

        if(output > m_outMax) output = m_outMax;
      else if(output < m_outMin) output = m_outMin;
        *m_output = output;

      /*Remember some variables for next time*/
      m_lastInput = input;
      m_lastTime = now;
        return true;
   }
   else return false;
}

/* SetTunings(...)*************************************************************
 * This function allows the controller's dynamic performance to be adjusted.
 * it's called automatically from the constructor, but tunings can also
 * be adjusted on the fly during normal operation
 ******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd, int POn)
{
   if (Kp<0 || Ki<0 || Kd<0) return;

   m_pOn = POn;
   m_pOnE = POn == P_ON_E;

   m_dispKp = Kp; m_dispKi = Ki; m_dispKd = Kd;

   double SampleTimeInSec = ((double)m_sampleTime)/1000;
   m_kp = Kp;
   m_ki = Ki * SampleTimeInSec;
   m_kd = Kd / SampleTimeInSec;

  if(m_controllerDirection ==REVERSE)
   {
      m_kp = (0 - m_kp);
      m_ki = (0 - m_ki);
      m_kd = (0 - m_kd);
   }
}

/* SetTunings(...)*************************************************************
 * Set Tunings using the last-rembered POn setting
 ******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd){
    SetTunings(Kp, Ki, Kd, m_pOn); 
}

/* SetSampleTime(...) *********************************************************
 * sets the period, in Milliseconds, at which the calculation is performed
 ******************************************************************************/
void PID::SetSampleTime(int NewSampleTime)
{
   if (NewSampleTime > 0)
   {
      double ratio  = (double)NewSampleTime
                      / (double)m_sampleTime;
      m_ki *= ratio;
      m_kd /= ratio;
      m_sampleTime = (unsigned long)NewSampleTime;
   }
}

/* SetOutputLimits(...)****************************************************
 *     This function will be used far more often than SetInputLimits.  while
 *  the input to the controller will generally be in the 0-1023 range (which is
 *  the default already,)  the output will be a little different.  maybe they'll
 *  be doing a time window and will need 0-8000 or something.  or maybe they'll
 *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
 *  here.
 **************************************************************************/
void PID::SetOutputLimits(double Min, double Max)
{
   if(Min >= Max) return;
   m_outMin = Min;
   m_outMax = Max;

   if(m_inAuto)
   {
       if(*m_output > m_outMax) *m_output = m_outMax;
       else if(*m_output < m_outMin) *m_output = m_outMin;

       if(m_outputSum > m_outMax) m_outputSum= m_outMax;
       else if(m_outputSum < m_outMin) m_outputSum= m_outMin;
   }
}

/* SetMode(...)****************************************************************
 * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
 * when the transition from manual to auto occurs, the controller is
 * automatically initialized
 ******************************************************************************/
void PID::SetMode(int Mode)
{
    bool newAuto = (Mode == AUTOMATIC);
    if(newAuto && !m_inAuto)
    {  /*we just went from manual to auto*/
        PID::Initialize();
    }
    m_inAuto = newAuto;
}

/* Initialize()****************************************************************
 *    does all the things that need to happen to ensure a bumpless transfer
 *  from manual to automatic mode.
 ******************************************************************************/
void PID::Initialize()
{
   m_outputSum = *m_output;
   m_lastInput = *m_input;
   if(m_outputSum > m_outMax) m_outputSum = m_outMax;
   else if(m_outputSum < m_outMin) m_outputSum = m_outMin;
}

/* SetControllerDirection(...)*************************************************
 * The PID will either be connected to a DIRECT acting process (+Output leads
 * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
 * know which one, because otherwise we may increase the output when we should
 * be decreasing.  This is called from the constructor.
 ******************************************************************************/
void PID::SetControllerDirection(int Direction)
{
   if(m_inAuto && Direction !=m_controllerDirection)
   {
      m_kp = (0 - m_kp);
      m_ki = (0 - m_ki);
      m_kd = (0 - m_kd);
   }
   m_controllerDirection = Direction;
}

/* Status Funcions*************************************************************
 * Just because you set the Kp=-1 doesn't mean it actually happened.  these
 * functions query the internal state of the PID.  they're here for display
 * purposes.  this are the functions the PID Front-end uses for example
 ******************************************************************************/
double PID::GetKp(){ return  m_dispKp; }
double PID::GetKi(){ return  m_dispKi;}
double PID::GetKd(){ return  m_dispKd;}
int PID::GetMode(){ return  m_inAuto ? AUTOMATIC : MANUAL;}
int PID::GetDirection(){ return m_controllerDirection;}